'''
if there is no receive device(only usbcanfd) , the ZCAN_MSG_INFO "txm" use txtype --2, self test
if there is a same baudrate receive device ,  the ZCAN_MSG_INFO "txm" use txtype --0, normal send

ZLG  Zhiyuan Electronics
'''

from ctypes import *
import threading
import time
import os

CUR_PATH = os.path.dirname(os.path.realpath(__file__))
dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
lib = cdll.LoadLibrary("./zlgcan.dll")


JOINT_CAN_ID = 3

ZCAN_DEVICE_TYPE = c_uint32
ZCAN_DEVICE_INDEX = c_uint32
ZCAN_CHANNEL = c_uint32
ZCAN_Reserved = c_uint32

canfd_test = 1

CAN_SELF_LOOP = 0

USBCANFD_200U = ZCAN_DEVICE_TYPE(43)
DEVICE_INDEX = ZCAN_DEVICE_INDEX(0)
CHANNEL_INDEX = ZCAN_CHANNEL(0)
Reserved = ZCAN_Reserved(0)


def input_thread():
    input()


# can/canfd messgae info
class ZCAN_MSG_INFO(Structure):
    _fields_ = [("txm", c_uint, 4),  # TXTYPE:0 normal,1 once, 2self
                ("fmt", c_uint, 4),  # 0-can2.0 frame,  1-canfd frame
                ("sdf", c_uint, 1),  # 0-data frame, 1-remote frame
                ("sef", c_uint, 1),  # 0-std_frame, 1-ext_frame
                ("err", c_uint, 1),  # error flag
                ("brs", c_uint, 1),  # bit-rate switch ,0-Not speed up ,1-speed up
                ("est", c_uint, 1),  # error state
                ("pad", c_uint, 19)]


# CAN Message Header
class ZCAN_MSG_HDR(Structure):
    _fields_ = [("ts", c_uint32),  # timestamp
                ("id", c_uint32),  # can-id
                ("info", ZCAN_MSG_INFO),
                ("pad", c_uint16),
                ("chn", c_uint8),  # channel
                ("len", c_uint8)]  # data length


# CAN2.0-frame
class ZCAN_20_MSG(Structure):
    _fields_ = [("msg_header", ZCAN_MSG_HDR),
                ("dat", c_ubyte * 8)]


# CANFD frame
class ZCAN_FD_MSG(Structure):
    _fields_ = [("msg_header", ZCAN_MSG_HDR),
                ("dat", c_ubyte * 64)]


class abit_config(Structure):
    _fields_ = [("tseg1", c_uint8),
                ("tseg2", c_uint8),
                ("sjw", c_uint8),
                ("smp", c_uint8),
                ("brp", c_uint16)]


class dbit_config(Structure):
    _fields_ = [("tseg1", c_uint8),
                ("tseg2", c_uint8),
                ("sjw", c_uint8),
                ("smp", c_uint8),
                ("brp", c_uint16)]


class ZCANFD_INIT(Structure):
    _fields_ = [("clk", c_uint32),
                ("mode", c_uint32),
                ("abit", abit_config),
                ("dbit", dbit_config)]


def canfd_start(Devicetype, DeviceIndex, Channel):
    canfd_init = ZCANFD_INIT()  # 1M+5M
    canfd_init.clk = 60000000
    canfd_init.mode = 0
    canfd_init.abit.tseg1 = 46
    canfd_init.abit.tseg2 = 11
    canfd_init.abit.sjw = 3
    canfd_init.abit.smp = 0
    canfd_init.abit.brp = 0
    canfd_init.dbit.tseg1 = 7
    canfd_init.dbit.tseg2 = 2
    canfd_init.dbit.sjw = 2
    canfd_init.dbit.smp = 0
    canfd_init.dbit.brp = 0
    ret = lib.VCI_InitCAN(Devicetype, DeviceIndex, Channel, byref(canfd_init))
    if ret == 0:
        print("VCI_InitCAN fail!")
    else:
        print("VCI_InitCAN success!")
    ret = lib.VCI_StartCAN(Devicetype, DeviceIndex, Channel)
    if ret == 0:
        print("VCI_StartCAN fail!")
    else:
        print("VCI_StartCAN success!")


def update_online():
    result = -1
    '''
    id = 0
    while id < 30:
        id += 1
        canfd_frame = (ZCAN_FD_MSG*1)()
        i = 0
        canfd_frame[i].msg_header.ts=0
        canfd_frame[i].msg_header.id=id
        if 1 == CAN_SELF_LOOP:
            canfd_frame[i].msg_header.info.txm = 2 #0--normal send, 2--self test
        else:
            canfd_frame[i].msg_header.info.txm = 0 #0--normal send, 2--self test
        canfd_frame[i].msg_header.info.fmt = 1 #canFD
        canfd_frame[i].msg_header.info.sdf = 0 #data frame
        canfd_frame[i].msg_header.info.sef = 0 #std frame
        canfd_frame[i].msg_header.info.err = 0
        canfd_frame[i].msg_header.info.brs = 1
        canfd_frame[i].msg_header.info.est = 0
        canfd_frame[i].msg_header.pad      = 0
        canfd_frame[i].msg_header.chn      = 0
        canfd_frame[i].msg_header.len      = 3

        canfd_frame[i].dat[0]=0x02
        canfd_frame[i].dat[1]=0x49
        canfd_frame[i].dat[2]=0x00

        ret= lib.VCI_TransmitFD(USBCANFD_200U,DEVICE_INDEX,CHANNEL_INDEX,byref(canfd_frame), 1)
        if 1 != ret :
            print("[E]update_online fail %d"%(ret))

        retry = 0
        while True:
            rcvfd_msgs  =(ZCAN_FD_MSG*1)()
            ret2 =lib.VCI_ReceiveFD(USBCANFD_200U,DEVICE_INDEX,CHANNEL_INDEX,byref(rcvfd_msgs),1,100)
            if 0 == ret2:
                time.sleep(0.1)
                retry += 1
                if retry > 10 :
                    print("[E]update_online no response overtime")
                    break
                continue
            for i in range(ret2):
                print("----update_online id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s"%(hex(rcvfd_msgs[i].msg_header.id),\
                        rcvfd_msgs[i].msg_header.info.brs,rcvfd_msgs[i].msg_header.len,''.join(hex(rcvfd_msgs[i].dat[j])+ ' 'for j in range(rcvfd_msgs[i].msg_header.len))))
            return
    '''
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 3

    canfd_frame[i].dat[0] = 0x02
    canfd_frame[i].dat[1] = 0x49
    canfd_frame[i].dat[2] = 0x00

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]update_online fail %d" % (ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]update_online no response overtime")
                break
            continue
        i = 0
        # print("----update_online id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s"%(hex(rcvfd_msgs[i].msg_header.id),\
        #         rcvfd_msgs[i].msg_header.info.brs,rcvfd_msgs[i].msg_header.len,''.join(hex(rcvfd_msgs[i].dat[j])+ ' 'for j in range(rcvfd_msgs[i].msg_header.len))))
        if rcvfd_msgs[i].msg_header.len != 3 and rcvfd_msgs[i].msg_header.id != (0x100 + JOINT_CAN_ID):
            return result

        return 0


def get_joint_state():
    result = -1
    position = 0
    current = 0
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID + 0x600
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 0

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]get_joints_state fail %d" % (ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]get_joints_state no response overtime")
                break
            continue
        if rcvfd_msgs[i].msg_header.len != 16 and rcvfd_msgs[i].msg_header.id != (0x700 + JOINT_CAN_ID):
            return result, position, current

        i = 0
        result = 0
        err_code = rcvfd_msgs[i].dat[0] + (rcvfd_msgs[i].dat[1] << 8)
        bat_v = rcvfd_msgs[i].dat[2] + (rcvfd_msgs[i].dat[3] << 8)
        tempure = rcvfd_msgs[i].dat[4] + (rcvfd_msgs[i].dat[5] << 8)
        state = rcvfd_msgs[i].dat[6]
        break_state = rcvfd_msgs[i].dat[7]
        position = rcvfd_msgs[i].dat[8] + (rcvfd_msgs[i].dat[9] << 8) + (rcvfd_msgs[i].dat[10] << 16) + (
                    rcvfd_msgs[i].dat[11] << 24)
        current = rcvfd_msgs[i].dat[12] + (rcvfd_msgs[i].dat[13] << 8) + (rcvfd_msgs[i].dat[14] << 16) + (
                    rcvfd_msgs[i].dat[15] << 24)

        print(
            f"----get_joints_state err:{err_code}, bat_v:{bat_v}, tempure:{tempure}, state:{state}, , break_state:{break_state}, position:{position}, current:{current}")
        # print("----get_joints_state id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s"%(hex(rcvfd_msgs[i].msg_header.id),\
        #         rcvfd_msgs[i].msg_header.info.brs,rcvfd_msgs[i].msg_header.len,''.join(hex(rcvfd_msgs[i].dat[j])+ ' 'for j in range(rcvfd_msgs[i].msg_header.len))))
        return result, position, current


def clear_joint_err():
    result = -1
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 3

    canfd_frame[i].dat[0] = 0x02
    canfd_frame[i].dat[1] = 0x0f
    canfd_frame[i].dat[2] = 0x01

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]clear_joint_err fail %d" % (ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]clear_joint_err no response overtime")
                break
            continue
        # print("----clear_joint_err id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s"%(hex(rcvfd_msgs[i].msg_header.id),\
        #         rcvfd_msgs[i].msg_header.info.brs,rcvfd_msgs[i].msg_header.len,''.join(hex(rcvfd_msgs[i].dat[j])+ ' 'for j in range(rcvfd_msgs[i].msg_header.len))))
        if rcvfd_msgs[i].msg_header.len != 3 and rcvfd_msgs[i].msg_header.id != (0x100 + JOINT_CAN_ID):
            return result

        return 0


def set_enable():
    result = -1
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 3

    canfd_frame[i].dat[0] = 0x02
    canfd_frame[i].dat[1] = 0x0a
    canfd_frame[i].dat[2] = 0x01

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]set_enable fail %d" % (ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]set_enable no response overtime")
                break
            continue
        # print("----set_enable id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s"%(hex(rcvfd_msgs[i].msg_header.id),\
        #         rcvfd_msgs[i].msg_header.info.brs,rcvfd_msgs[i].msg_header.len,''.join(hex(rcvfd_msgs[i].dat[j])+ ' 'for j in range(rcvfd_msgs[i].msg_header.len))))
        if rcvfd_msgs[i].msg_header.len != 3 and rcvfd_msgs[i].msg_header.id != (0x100 + JOINT_CAN_ID):
            return result

        return 0


def set_joint_zero():
    result = -1
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 3

    canfd_frame[i].dat[0] = 0x02
    canfd_frame[i].dat[1] = 0x0e
    canfd_frame[i].dat[2] = 0x01

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]set_joint_zero fail %d" % (ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]set_joint_zero no response overtime")
                break
            continue
        # print("----set_joint_zero id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s"%(hex(rcvfd_msgs[i].msg_header.id),\
        #         rcvfd_msgs[i].msg_header.info.brs,rcvfd_msgs[i].msg_header.len,''.join(hex(rcvfd_msgs[i].dat[j])+ ' 'for j in range(rcvfd_msgs[i].msg_header.len))))
        if rcvfd_msgs[i].msg_header.len != 3 and rcvfd_msgs[i].msg_header.id != (0x100 + JOINT_CAN_ID):
            return result

        return 0


# mode is 1(current), 2(velocity), 3(position)
def set_work_mode(mode):
    if mode != 1 and mode != 2 and mode != 3:
        print("[E]%s mode is not 1, 2, 3 ")
        return

    result = -1
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 3

    canfd_frame[i].dat[0] = 0x02
    canfd_frame[i].dat[1] = 0x30
    canfd_frame[i].dat[2] = mode

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]set_work_mode %d fail %d" % (mode, ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]set_work_mode  %d no response overtime" % (mode))
                break
            continue
        # print("----set_joint_zero id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s"%(hex(rcvfd_msgs[i].msg_header.id),\
        #         rcvfd_msgs[i].msg_header.info.brs,rcvfd_msgs[i].msg_header.len,''.join(hex(rcvfd_msgs[i].dat[j])+ ' 'for j in range(rcvfd_msgs[i].msg_header.len))))
        if rcvfd_msgs[i].msg_header.len != 3 and rcvfd_msgs[i].msg_header.id != (0x100 + JOINT_CAN_ID):
            return result

        return 0


def position_control(value):
    print(f"----position_control new value:{value}")
    result = -1
    cur_current = 0
    cur_vel = 0
    last_pos = 0
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID + 0x200
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 4

    canfd_frame[i].dat[0] = value & 0xff
    canfd_frame[i].dat[1] = (value >> 8) & 0xff
    canfd_frame[i].dat[2] = (value >> 16) & 0xff
    canfd_frame[i].dat[3] = (value >> 24) & 0xff

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]position_control %d fail %d" % (value, ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]position_control no response overtime")
                return result, cur_current, cur_vel, last_pos
            continue
        i = 0
        if rcvfd_msgs[i].msg_header.len != 16 and rcvfd_msgs[i].msg_header.id != (0x500 + JOINT_CAN_ID):
            print(f"[E]position_control date err len:{rcvfd_msgs[i].msg_header.len}, id:{rcvfd_msgs[i].msg_header.id},")
            return result, cur_current, cur_vel, last_pos

        result = 0
        cur_current = rcvfd_msgs[i].dat[0] + (rcvfd_msgs[i].dat[1] << 8) + (rcvfd_msgs[i].dat[2] << 16) + (
                    rcvfd_msgs[i].dat[3] << 24)
        cur_vel = rcvfd_msgs[i].dat[4] + (rcvfd_msgs[i].dat[5] << 8) + (rcvfd_msgs[i].dat[6] << 16) + (
                    rcvfd_msgs[i].dat[7] << 24)
        last_pos = rcvfd_msgs[i].dat[8] + (rcvfd_msgs[i].dat[9] << 8) + (rcvfd_msgs[i].dat[10] << 16) + (
                    rcvfd_msgs[i].dat[11] << 24)
        state = rcvfd_msgs[i].dat[12] + (rcvfd_msgs[i].dat[13] << 8)
        errcode = rcvfd_msgs[i].dat[14] + (rcvfd_msgs[i].dat[15] << 8)

        print(
            f"----position_control cur_current:{cur_current}, cur_vel:{cur_vel}, last_pos:{last_pos}, state:{state}, errcode:{errcode}")
        return result, cur_current, cur_vel, last_pos


def current_control(value):
    result = -1
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID + 0x400
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 4

    canfd_frame[i].dat[0] = value & 0xff
    canfd_frame[i].dat[1] = (value >> 8) & 0xff
    canfd_frame[i].dat[2] = (value >> 16) & 0xff
    canfd_frame[i].dat[3] = (value >> 24) & 0xff

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]current_control %d fail %d" % (value, ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]current_control %d no response overtime" % (value))
                break
            continue
        i = 0
        if rcvfd_msgs[i].msg_header.len != 16 and rcvfd_msgs[i].msg_header.id != (0x500 + JOINT_CAN_ID):
            print(f"[E]current_control date err len:{rcvfd_msgs[i].msg_header.len}, id:{rcvfd_msgs[i].msg_header.id},")
            return result, cur_current, cur_vel, last_pos

        result = 0
        cur_current = rcvfd_msgs[i].dat[0] + (rcvfd_msgs[i].dat[1] << 8) + (rcvfd_msgs[i].dat[2] << 16) + (
                    rcvfd_msgs[i].dat[3] << 24)
        cur_vel = rcvfd_msgs[i].dat[4] + (rcvfd_msgs[i].dat[5] << 8) + (rcvfd_msgs[i].dat[6] << 16) + (
                    rcvfd_msgs[i].dat[7] << 24)
        last_pos = rcvfd_msgs[i].dat[8] + (rcvfd_msgs[i].dat[9] << 8) + (rcvfd_msgs[i].dat[10] << 16) + (
                    rcvfd_msgs[i].dat[11] << 24)
        state = rcvfd_msgs[i].dat[12] + (rcvfd_msgs[i].dat[13] << 8)
        errcode = rcvfd_msgs[i].dat[14] + (rcvfd_msgs[i].dat[15] << 8)

        print(
            f"----current_control cur_current:{cur_current}, cur_vel:{cur_vel}, last_pos:{last_pos}, state:{state}, errcode:{errcode}")
        return result, cur_current, cur_vel, last_pos


def velocity_control(value):
    result = -1
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID + 0x300
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 4

    canfd_frame[i].dat[0] = value & 0xff
    canfd_frame[i].dat[1] = (value >> 8) & 0xff
    canfd_frame[i].dat[2] = (value >> 16) & 0xff
    canfd_frame[i].dat[3] = (value >> 24) & 0xff

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]velocity_control %d fail %d" % (value, ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]velocity_control %d no response overtime" % (value))
                break
            continue
        i = 0
        if rcvfd_msgs[i].msg_header.len != 16 and rcvfd_msgs[i].msg_header.id != (0x500 + JOINT_CAN_ID):
            print(f"[E]velocity_control date err len:{rcvfd_msgs[i].msg_header.len}, id:{rcvfd_msgs[i].msg_header.id},")
            return result, cur_current, cur_vel, last_pos

        result = 0
        cur_current = rcvfd_msgs[i].dat[0] + (rcvfd_msgs[i].dat[1] << 8) + (rcvfd_msgs[i].dat[2] << 16) + (
                    rcvfd_msgs[i].dat[3] << 24)
        cur_vel = rcvfd_msgs[i].dat[4] + (rcvfd_msgs[i].dat[5] << 8) + (rcvfd_msgs[i].dat[6] << 16) + (
                    rcvfd_msgs[i].dat[7] << 24)
        last_pos = rcvfd_msgs[i].dat[8] + (rcvfd_msgs[i].dat[9] << 8) + (rcvfd_msgs[i].dat[10] << 16) + (
                    rcvfd_msgs[i].dat[11] << 24)
        state = rcvfd_msgs[i].dat[12] + (rcvfd_msgs[i].dat[13] << 8)
        errcode = rcvfd_msgs[i].dat[14] + (rcvfd_msgs[i].dat[15] << 8)

        print(
            f"----velocity_control cur_current:{cur_current}, cur_vel:{cur_vel}, last_pos:{last_pos}, state:{state}, errcode:{errcode}")
        return result, cur_current, cur_vel, last_pos


def get_regiter_data(reg):
    result = -1
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 3

    canfd_frame[i].dat[0] = 0x01
    canfd_frame[i].dat[1] = reg
    canfd_frame[i].dat[2] = 2

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]set_enable fail %d" % (ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]set_enable no response overtime")
                break
            continue
        i = 0
        # print("----get_regiter_data id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s"%(hex(rcvfd_msgs[i].msg_header.id),\
        #         rcvfd_msgs[i].msg_header.info.brs,rcvfd_msgs[i].msg_header.len,''.join(hex(rcvfd_msgs[i].dat[j])+ ' 'for j in range(rcvfd_msgs[i].msg_header.len))))
        if rcvfd_msgs[i].msg_header.len != 4 and rcvfd_msgs[i].msg_header.id != (0x100 + JOINT_CAN_ID):
            print(f"[E]get_regiter_data date err len:{rcvfd_msgs[i].msg_header.len}, id:{rcvfd_msgs[i].msg_header.id},")
            return result, 0

        result = 0
        value = rcvfd_msgs[i].dat[2] + (rcvfd_msgs[i].dat[3] << 8)
        return result, value


def set_regiter_data(reg, value):
    result = -1
    canfd_frame = (ZCAN_FD_MSG * 1)()
    i = 0
    canfd_frame[i].msg_header.ts = 0
    canfd_frame[i].msg_header.id = JOINT_CAN_ID
    if 1 == CAN_SELF_LOOP:
        canfd_frame[i].msg_header.info.txm = 2  # 0--normal send, 2--self test
    else:
        canfd_frame[i].msg_header.info.txm = 0  # 0--normal send, 2--self test
    canfd_frame[i].msg_header.info.fmt = 1  # canFD
    canfd_frame[i].msg_header.info.sdf = 0  # data frame
    canfd_frame[i].msg_header.info.sef = 0  # std frame
    canfd_frame[i].msg_header.info.err = 0
    canfd_frame[i].msg_header.info.brs = 1
    canfd_frame[i].msg_header.info.est = 0
    canfd_frame[i].msg_header.pad = 0
    canfd_frame[i].msg_header.chn = 0
    canfd_frame[i].msg_header.len = 4

    canfd_frame[i].dat[0] = 0x02
    canfd_frame[i].dat[1] = reg
    canfd_frame[i].dat[2] = value & 0xff
    canfd_frame[i].dat[3] = (value >> 8) & 0xff

    ret = lib.VCI_TransmitFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(canfd_frame), 1)
    if 1 != ret:
        print("[E]set_regiter_data %d=%d transmit fail %d" % (reg, value, ret))

    retry = 0
    while True:
        rcvfd_msgs = (ZCAN_FD_MSG * 1)()
        ret2 = lib.VCI_ReceiveFD(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX, byref(rcvfd_msgs), 1, 100)
        if 0 == ret2:
            time.sleep(0.1)
            retry += 1
            if retry > 10:
                print("[E]set_regiter_data %d=%d no response" % (reg, value))
                break
            continue
        i = 0
        print("----get_regiter_data id:%s , type: canfd  ,brs:%d ,dlc:%d ,data:%s" % (hex(rcvfd_msgs[i].msg_header.id), \
                                                                                      rcvfd_msgs[i].msg_header.info.brs,
                                                                                      rcvfd_msgs[i].msg_header.len,
                                                                                      ''.join(hex(
                                                                                          rcvfd_msgs[i].dat[j]) + ' '
                                                                                              for j in range(rcvfd_msgs[
                                                                                                                 i].msg_header.len))))
        if rcvfd_msgs[i].msg_header.len != 3 and rcvfd_msgs[i].msg_header.id != (0x100 + JOINT_CAN_ID):
            print("[E]set_regiter_data %d=%d len err or id err" % (reg, value))
            return result

        result = 0
        time.sleep(0.01)
        return result


zero_pos = 1575150

if __name__ == "__main__":
    ret = lib.VCI_OpenDevice(USBCANFD_200U, DEVICE_INDEX, Reserved)
    if ret == 0:
        print("Opendevice fail!")
    else:
        print("Opendevice success!")

    canfd_start(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX)

    # 更新在线标志位
    if 0 != update_online():
        print("update_online fail")
        exit

    # 获取错误码
    ret, value = get_regiter_data(0x04)
    if 0 == ret:
        print(f"上次的错误码:%x" % (value))

    # 清除错误
    clear_joint_err()

    # 获取关节状态和关节零位
    ret, current_pos, current = get_joint_state()
    print(f"当前的positon值:[{zero_pos}, {current_pos}]")
    cur_angle = (current_pos - zero_pos) / 10000
    print(f"当前角度值:{cur_angle}")

    # set_regiter_data(0x44, 0)
    # set_regiter_data(0x45, 0)
    # set_regiter_data(0x46, 0xffff)
    # set_regiter_data(0x47, 0xffff)

    # set_regiter_data(0x36, 0)
    # set_regiter_data(0x37, 0x3000)

    # 获取关节的限位信息
    limit_min = 0
    limit_max = 0
    ret, value = get_regiter_data(0x44)
    if 0 == ret:
        limit_min = value
    ret, value = get_regiter_data(0x45)
    if 0 == ret:
        limit_min = limit_min + (value << 16)
    ret, value = get_regiter_data(0x46)
    if 0 == ret:
        limit_max = value
    ret, value = get_regiter_data(0x47)
    if 0 == ret:
        limit_max = limit_min + (value << 16)
    print(f"限位[{limit_min}, {limit_max}]")

    # 设置关节零位
    if 0 != set_joint_zero():
        print(f"set_joint_zero fail")

    # 位置控制
    loop_times = 0
    loop_seconds = 10
    loop_period = 0.01
    set_work_mode(3)

    # 设置使能
    set_enable()

    # 跑到一个固定的位置
    while True:
        # 开始运动
        current_pos -= 10000
        position_control(429490000)
        time.sleep(20000)

    step1_target_angle = 90
    step2_target_angle = -90
    cur_step = 1
    if cur_angle > 90:
        cur_step = 2
    else:
        cur_step = 1
    start_run = 0
    position_step = 1
    next_angle = 0
    while True:
        # 开始运动
        if 0 == start_run:
            start_run = 1
            # 判断状态
            if 1 == cur_step:
                next_angle = cur_angle + position_step
                if next_angle >= 90:
                    next_angle = 90
                    cur_step = 2
            else:
                next_angle = cur_angle - position_step
                if next_angle <= -90:
                    next_angle = -90
                    cur_step = 1
            position_control(int(next_angle * 10000))

        # 开始check是否到位置
        ret, current_pos, current = get_joint_state()
        cur_angle = (current_pos - zero_pos) / 10000
        print(f"当前角度值:{cur_angle}")
        if abs(cur_angle - next_angle) < 0.1:
            start_run = 1
        # sleep loop_period
        time.sleep(loop_period)

    ret = lib.VCI_ResetCAN(USBCANFD_200U, DEVICE_INDEX, CHANNEL_INDEX)
    if ret == 0:
        print("ResetCAN Failed!")
    else:
        print("ResetCAN success!")

    ret = lib.VCI_CloseDevice(USBCANFD_200U, DEVICE_INDEX)
    if ret == 0:
        print("Closedevice Failed!")
    else:
        print("Closedevice success!")
    print('done')
